# -*- coding: utf-8 -*-

import os
import struct
import queue
import serial 
import copy
import yaml
import math
from timeit import default_timer as timer
import serial.tools.list_ports
from datetime import datetime
from loguru import logger as log
from PyQt5 import QtWidgets
from PyQt5.QtCore import QObject, QTimer, QThread, pyqtSignal
from tcp_server import *
from main_dialog import Ui_MainWindow


class Database:
    """
        一个简单的属性集合
    """
    def __init__(self):
        self._dict = {}

    def set(self, id, value):
        self._dict[id] = value 
    
    def get(self, id, default_value=None):
        ret = ""
        try:
            ret = self._dict[id]
        except Exception as e:
            return default_value 
        return ret 

class UiLog(QThread):
    """
    日志子模块，处理日志事务
    """
    messageReceived = pyqtSignal(str)
    
    def __init__(self, logToFile=False):
        super(UiLog, self).__init__()
        self.running = True
        self.msgq = queue.Queue()
        self.logToFile = logToFile
        self.logFp = None        
        if self.logToFile:
            now = datetime.now().strftime('%Y%m%d%H%M%S')
            logFile = "log\\log_%s.log" % now 
            if not os.path.exists("log"):
                os.mkdir("log")    
            try:    
                self.logFp = open(logFile, "w") 
            except Exception as e:
                print(e)

    def __del__(self):
        self.running = False
        self.wait()
        if self.logFp:
            self.logFp.close()
            
    def log(self, msg):
        now = datetime.now().strftime('%H:%M:%S.%f')[:-3]
        self.msgq.put("[%s] %s" % (now, msg))

    # 重载运行函数
    def run(self):
        while self.running == True:
            try:
                msg = self.msgq.get(block=True, timeout=0.1)
            except:
                continue  
            self.messageReceived.emit(msg)
            # 记录到文件
            if self.logToFile:
                # now = datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]
                # msg = "[%s] %s\n" % (now, msg)
                self.logFp.write(msg + "\n")
                self.logFp.flush()


class Setting(QObject):
    """设置模块
    内置一份默认配置
    初始化：
       如果指定了路径，构造函数中尝试从文件加载配置， 否则使用默认配置
    恢复到默认配置:
      如果指定了路径，使用默认配置写入到配置文件中
    
    获取配置：
    修改配置：

    """
    serialChanged = pyqtSignal()

    # 默认配置
    defaultConfig = {
        "serial" : { 
        "port": "COM1", 
        "baudrate": '115200', 
        "supported_baudrates": ['57600', '115200', '230400', '460800', '500000', '576000', '921600', '1000000', '1152000', '1500000']
        },
        "pid" : {
            "ranges" : [
                {"kx_min": 0.000, "kx_max": 2.000,"kx_step": 0.001, "limit_min": 0.4, "limit_max": 2.0, "limit_step": 0.001, "control_max": 0.6},
                {"kx_min": 0.000, "kx_max": 10.000, "kx_step": 0.010, "limit_min": 10000, "limit_max": 30000, "limit_step": 100, "control_max" : 25000},
            ],
            "options" : [
                {"name":"左右轮", "range": 0, "target": "both_wheel"},                 
                {"name":"左轮", "range": 0, "target": "left_wheel"}, 
                {"name":"右轮", "range": 0, "target": "right_wheel"}, 
                {"name":"风机", "range": 1, "target": "fan"}
            ],
        },
        "visual": {
            "port" : 3210,
            "period" : 100,
            "options": [
                {"id": 0, "name": "不使用"},
                {"id": 111, "name": "左轮速度"},
                {"id": 117, "name": "左轮PidTarget"},
                {"id": 119, "name": "左轮PidOutput"},
                {"id": 118, "name": "左轮PidError"},
                {"id": 112, "name": "左轮电流"},
                {"id": 121, "name": "右轮速度"},
                {"id": 127, "name": "右轮PidTarget"},
                {"id": 129, "name": "右轮PidOutput"},
                {"id": 128, "name": "右轮PidError"},
                {"id": 122, "name": "右轮电流"},
                {"id": 171, "name": "风机转速"},
                {"id": 190, "name": "风机PidTarget"},
                {"id": 191, "name": "风机PidOutput"},
                {"id": 192, "name": "风机PidError"},
                {"id": 172, "name": "风机电流"},
                {"id": 141, "name": "主刷电流"},
                {"id": 143, "name": "左边刷电流"},
                {"id": 145, "name": "右边刷电流"},
                {"id": 147, "name": "水泵电流"},
                {"id": 180, "name": "前地检"},
                {"id": 181, "name": "左地检"},
                {"id": 182, "name": "右地检"},
                {"id": 103, "name": "电池电压"},
                {"id": 150, "name": "IMU角速度X"},
                {"id": 151, "name": "IMU角速度Y"},
                {"id": 152, "name": "IMU角速度Z"},
                {"id": 153, "name": "IMU加速度X"},
                {"id": 154, "name": "IMU加速度Y"},
                {"id": 155, "name": "IMU加速度Z"},
                {"id": 160, "name": "里程计坐标X"},
                {"id": 161, "name": "里程计坐标Y"},
                {"id": 162, "name": "里程计坐标Z"},
                {"id": 163, "name": "里程计YAW"},
                {"id": 164, "name": "里程计PITCH"},
                {"id": 165, "name": "里程计ROLL"},
            ],
            "ch1": 0, "ch2": 0, "ch3": 0, "ch4": 0, 
            "ch5": 0, "ch6": 0, "ch7": 0, "ch8": 0, 
        }
    }
    
    def __init__(self, path=None):
        super(Setting, self).__init__()
        self._path = path
        self._timer = QTimer()
        self._timer.timeout.connect(self.__timerHandle)
        self.ports = self.getSerialPorts()
        self._timer.start(1000)
        self._dict = {}
        # 如果有指定文件，先从文件加载配置
        if self._path:
            try:
                with open(self._path, encoding="utf-8") as fp:
                    self._dict = yaml.safe_load(fp)
            except Exception as e:
                log.warning("unabled to open setting file(%s)" % self._path)
        
        if len(self._dict) == 0:
            log.info("load default setting")
            self._dict = copy.deepcopy(self.defaultConfig)
        else:
            log.info("load setting from %s" % self._path)

    def save(self):
        """如果指定了路径，将配置保存到文件
        """
        if self._path:
            f = open(self._path, "w", encoding="utf-8")
            yaml.dump(self._dict, f, encoding='utf-8', allow_unicode=True)
            f.close()
            log.info("setting saved to '%s'" % self._path)

    def get(self)->dict:
        """
        获取配置字典
        """
        return copy.deepcopy(self._dict)

    def __timerHandle(self):
        nowPorts = self.getSerialPorts()
        ## 因为返回的是排序后的列表，所有可以按顺序比较 
        if len(nowPorts) != len(self.ports):
            self.serialChanged.emit()
        elif len(nowPorts) > 0:
            for p in range(len(nowPorts)):
                if nowPorts[p] != self.ports[p]:
                    self.serialChanged.emit()
        self.ports = nowPorts

    def getSerialConfig(self):
        port = 'COM1'
        rate = '115200'
        try:
            port = self._dict['serial']['port']
        except Exception as e:
            pass 
        try:
            rate = self._dict['serial']['baudrate']
        except Exception as e:
            pass 
        if len(self.ports) > 0 and (port not in self.ports):
            port = self.ports[0]
        if rate not in self.getSerialBaudrates():
            rate = self.getSerialBaudrates()[0]
        return str(port), str(rate) 
    
    def setSerialConfig(self, port=None, rate=None):
        if port:
            log.info("set serial port to %s" % port)
            self._dict['serial']['port'] = port 
        if rate:
            log.info("set serial baudrate to %s" % rate)
            self._dict['serial']['baudrate'] = rate 
        self.save()

    def getSerialPorts(self)->list: 
        """获取串口列表

        Returns:
            list: 返回一个串口列表的名称
        """
        ports = list(serial.tools.list_ports.comports())
        names = []
        for p in ports:
            s = str(p).split('-')[0]
            if s.startswith("COM"):
                names.append(s.strip())
        return sorted(names)
    
    def getSerialBaudrates(self)->list:
        """获取支持的波特率列表

        Returns:
            list: 返回支持的波特率列表
        """
        rates = self.defaultConfig['serial']['supported_baudrates']
        try:
            rates = self._dict['serial']['supported_baudrates']
        except Exception as e:
            pass 
        return copy.deepcopy(rates) 
    
    def getPidTargets(self)->list:
        options = self.defaultConfig['pid']['options']
        try:
            options = self._dict['pid']['options']
        except Exception as e:
            pass 
        return copy.deepcopy(options)
    
    def getPidRanges(self)->list:
        ranges = self.defaultConfig['pid']['ranges']
        try:
            ranges = self._dict['pid']['ranges']
        except Exception as e:
            pass 
        return copy.deepcopy(ranges)

    def getDataVisualIpPort(self):
        port = self.defaultConfig['visual']['port']
        try:
            port = self._dict['visual']['port']
        except Exception as e:
            pass 
        return port 

    def getDataVisualTxPeriod(self):
        period = self.defaultConfig['visual']['period']
        try:
            period = self._dict['visual']['period']
        except Exception as e:
            pass 
        return period 

    def getDataVisualOptions(self):
        """
        以列表形返回ID及名称
        """
        options = self.defaultConfig['visual']['options']
        try:
            options = self._dict['visual']['options']
        except Exception as e:
            pass 
        ids = []
        names = []
        for opt in options:
            ids.append(opt['id'])
            names.append(opt['name'])
        return ids,names 

    def getDataVisualChannelConfigs(self):
        chs = []
        for ch in range(8):
            ch_name = "ch%d" % (ch + 1)
            v = self.defaultConfig['visual'][ch_name]
            try:
                v = self._dict['visual'][ch_name]
            except Exception as e:
                pass 
            chs.append(v)
        return chs        

    def setDataVisualConfig(self, port, period, chs:list):
        log.info("set ip port:%d, period:%d" % (port, period))
        self._dict['visual']['port'] = port
        self._dict['visual']['period'] = period
        self._dict['visual']['ch1'] = chs[0]
        self._dict['visual']['ch2'] = chs[1]
        self._dict['visual']['ch3'] = chs[2]
        self._dict['visual']['ch4'] = chs[3]
        self._dict['visual']['ch5'] = chs[4]
        self._dict['visual']['ch6'] = chs[5]
        self._dict['visual']['ch7'] = chs[6]
        self._dict['visual']['ch8'] = chs[7]
        self.save()

class Indicator(QObject):
    """
        指示灯，支持显示指定的状态，或闪烁显示
    """
    INACTIVE = 0
    WARNING = 1
    ACTIVE = 2
    DISMISS = 3
    def __init__(self, obj, reverse=False):
        super(Indicator, self).__init__()
        self._object = obj
        self._size = obj.minimumHeight()
        self._timer = QTimer()
        self._timer.timeout.connect(self.__flashHandle)
        self._state = self.INACTIVE
        self._flashBaseState = self.DISMISS
        self._flashState = self.INACTIVE
        self._reverse = reverse
        self.show(self.INACTIVE)

    def show(self, state):
        self._state = state
        colors = ['red', 'orange', 'green', "rgb(240,240,240)"]
        if self._reverse:
            colors = ['green', 'orange', 'red', "rgb(240,240,240)"]
        self._object.setStyleSheet("QPushButton{background-color:%s;border:0;border-radius: %dpx;}" % (colors[state], self._size / 2))
        
    def active(self):
        self._timer.stop()
        self.show(self.ACTIVE)

    def inactive(self):
        self._timer.stop()
        self.show(self.INACTIVE)

    def warning(self):
        self._timer.stop()
        self.show(self.WARNING)
    
    def dismiss(self):
        self._timer.stop()
        self.show(self.DISMISS)
    
    def flash(self, state, period, baseState=DISMISS):
        if state == baseState:
            return 
        self._flashState = state 
        self._flashBaseState = baseState
        self.show(state)
        self._timer.start(period)

    def __flashHandle(self):
        if self._state == self._flashBaseState:
            self.show(self._flashState)
        else:
            self.show(self._flashBaseState)


class PwmRegultor(QObject):
    """PWM调节器

    Args:
        QObject (_type_): _description_

    Returns:
        _type_: _description_
    """
    ## 值改变ID
    valueChanged = pyqtSignal(int, int)
        
    def __init__(self, slider:QtWidgets.QSlider, label:QtWidgets.QLabel, attrId):
        super(PwmRegultor, self).__init__()
        self._slider = slider
        self._label = label
        self._attrId = attrId  
        self._slider.setRange(0, 100)
        self._slider.setValue(0)
        self._slider.setSingleStep(1)
        self._slider.valueChanged.connect(self.__onValueChanged)
        self._label.setText("%d%%" % 0)

    def __onValueChanged(self, value=0):        
        self._label.setText("%d%%" % value)
        self.valueChanged.emit(self._attrId, value)        
        
    def getValue(self):
        return self._slider.value()
    

class SimpleRegultor(QObject):
    """PWM调节器

    Args:
        QObject (_type_): _description_

    Returns:
        _type_: _description_
    """
    ## 值改变ID
    valueChanged = pyqtSignal(int, int)
        
    def __init__(self, slider:QtWidgets.QSlider, label:QtWidgets.QLabel, attrId):
        super(PwmRegultor, self).__init__()
        self._slider = slider
        self._label = label
        self._attrId = attrId  
        self._slider.setRange(0, 100)
        self._slider.setValue(0)
        self._slider.setSingleStep(1)
        self._slider.valueChanged.connect(self.__onValueChanged)
        self._label.setText("0%%")

    def __onValueChanged(self, value):        
        self._label.setText("%d%%" % value)
        self.valueChanged.emit(self._attrId, value)        
        
    def getValue(self):
        return self._slider.value()    

class GeneralRegultor(QObject):
    """通用slider调节器

    Args:
        QObject (_type_): _description_

    Returns:
        _type_: _description_
    """
    ## 值改变ID
    valueChanged = pyqtSignal(int, float)
        
    def __init__(self, 
                 slider:QtWidgets.QSlider, label:QtWidgets.QLabel, 
                 buttonPlus:QtWidgets.QPushButton = None, buttonMinus:QtWidgets.QPushButton = None,
                 id:int = 0, real_min=0.0, real_max=100.0, real_step=1.0, reset_value=None):
        super(GeneralRegultor, self).__init__()
        self._slider = slider
        self._label = label
        self._buttonPlus = buttonPlus
        self._buttonMinus = buttonMinus
        self._id = id
        self._min = real_min
        self._max = real_max
        self._step = real_step
        self._slider.setRange(0, 100)
        self._slider.setValue(0)            
        self._slider.setSingleStep(1)
        self._reset_value = self._min
                            
        if not reset_value and reset_value != 0:
            # 仅为空
            pass 
        else:
            self._reset_value = reset_value
                      
        self._slider.valueChanged.connect(self.__onValueChanged)
        #
        if self._buttonPlus:
            self._buttonPlus.clicked.connect(self.__onButtonPlusClicked)
        if self._buttonMinus:
            self._buttonMinus.clicked.connect(self.__onButtonMinusClicked)
        
        self.reset()              

    def __onButtonPlusClicked(self):
        if self._real_value < self._max:
            self._real_value += self._step
            if self._real_value > self._max:
                self._real_value = self._max             
            self.__updateLabel()
            self.valueChanged.emit(self._id, self._real_value)
            sv = ((self._real_value - self._min) * 100) / (self._max - self._min) 
            #log.info("n:%d s:%d" % (int(sv), self._slider.value()))
            if int(sv) != self._slider.value():                
                self._slider.setValue(int(sv))

    def __onButtonMinusClicked(self):
        if self._real_value > self._min:     
            self._real_value = self._real_value - self._step          
            if self._real_value < self._min:
                self._real_value = self._min 
            self.__updateLabel()
            self.valueChanged.emit(self._id, self._real_value)
            ## 减法 会有问题，回滚到下一阶段，导致值不对
            sv = ((self._real_value - self._min) * 100) / (self._max - self._min) 
            rv =  (int(sv) * (self._max - self._min)) / 100 + self._min             
            #log.info("new:%d %.3f s:%d" % (int(sv), rv, self._slider.value()))
            if self._real_value <= rv:
                self._slider.setValue(int(sv))

    def __onValueChanged(self, value):        
        sv = self._slider.value()
        self._real_value = (sv * (self._max - self._min)) / 100 + self._min 
        self.__updateLabel()
        self.valueChanged.emit(self._id, self._real_value)

    def __updateLabel(self):
        ## 根据step调整显示精度, 
        # 如果Step < 0.01 使用3位精度
        # 如果step < 0.1 使用2位精度 
        # 如果step < 1 使用1位精度 
        # 如果step 其他使用整数显示 
        if self._step < 0.01:
            self._label.setText("%.3f" % self._real_value)
        elif self._step < 0.1:
            self._label.setText("%.2f" % self._real_value)
        elif self._step < 1.0:
            self._label.setText("%.1f" % self._real_value)
        else:
            self._label.setText("%.0f" % self._real_value)        
        
    def getValue(self)->float:      
        return self._real_value  
    
    def setValue(self, rv):
        sv = 0
        if rv > self._max:
            sv = 100
        elif rv < self._min:
            sv = 0
        else:
            sv = ((rv - self._min) * 100) / (self._max - self._min)        
        self._slider.setValue(sv)

    def reset(self):
        self._real_value = self._reset_value
        self.__updateLabel()
        self.setValue(self._real_value)        

class ExtButton(QObject):
    """
    重新封装一个函数，支持ID及长按事件
    """    
    EVENT_PRESSED = 65536
    EVENT_RELEASED = 65537
    EVENT_LONG_RELEASED = 65538
    ##其他为长按事件，表示长按秒数
    
    ## 按键事件: 分别为ID，事件     
    keyEvent = pyqtSignal(int, int)    
    
    def __init__(self, obj:QtWidgets.QPushButton, id:int):
        super(ExtButton, self).__init__()
        self._id = id
        self._obj = obj
        self._timer = QTimer()
        self._timer.timeout.connect(self.__onTimer)        
        self._obj.pressed.connect(self.__onPressed)
        self._obj.released.connect(self.__onReleased)
        self._pressedSeconds = 0
                
    def __onPressed(self):
        self.keyEvent.emit(self._id, self.EVENT_PRESSED)
        self._timer.start(1000)
        self._pressedSeconds = 0
    
    def __onReleased(self):        
        self._timer.stop()
        if self._pressedSeconds > 0:
            self.keyEvent.emit(self._id, self.EVENT_LONG_RELEASED)
        else:
            self.keyEvent.emit(self._id, self.EVENT_RELEASED)
        self._pressedSeconds = 0
    
    def __onTimer(self):
        self._pressedSeconds += 1
        self.keyEvent.emit(self._id, self._pressedSeconds)    

class IpRemoter(TcpServer, QObject):
    """
    网络遥控器小程序对接类，继承自TCPServer
    """
    commandReceived = pyqtSignal(str)
    def __init__(self, ip='0.0.0.0', port=7654, uiLog:UiLog=None):
        TcpServer.__init__(self, ip, port)
        QObject.__init__(self)
        self.uiLog = uiLog

    def log(self, msg):
        if self.uiLog:
            self.uiLog.log(msg)    

    def on_start(self):
        self.log("启动网络遥控器小程序服务@%s" % self.info())

    def on_client_connect(self, client: Client):
        self.add_client(client)
        self.log("网络遥控器(%s)连接成功" % client.info())

    def on_client_disconnect(self, client):
        self.remove_client(client)
        self.log("网络遥控器(%s)连接断开" % client.info())

    def on_receive(self, client: Client, data: bytes):
        ## 小程序的数据格式为"remote #xxxx\n"
        log.debug("Remote command from (%s): %s" % (client.info(), str(data)))
        msg = data.decode().strip()
        if msg.startswith("remote"):
            self.commandReceived.emit(msg[7:])



class VofaServer(TcpServer, QObject):
    """
    使用定时器来实现间隔发送，可以实现10ms的间隔
    
    """

    def __init__(self, db:Database, attrIds=[], uiLog:UiLog=None, ip='0.0.0.0', port=3210, period=1000):
        TcpServer.__init__(self, ip, port)
        QObject.__init__(self)
        self.db = db
        self.uiLog = uiLog
        self.period = period
        self.attrIds = []
        #self.thread = threading.Thread(target=self.vofa_server_main)
        self.timer = QTimer()
        self.timer.timeout.connect(self.timer_handle)
        self.transmiting = False
        self.begin = True
        self.data_available = False
        for v in attrIds:
            if v > 0:
                self.attrIds.append(v)

        self.test_counter1 = 0.0
        self.STATE_OK = 0
        self.STATE_ATTRIBUTE_NOT_CONFIGURED = 1
        self.STATE_VOFA_CLIENT_NOT_AVAILABLE = 2
        self.STATE_DATA_NOY_AVAILABLE = 3


    def get_state(self):
        if self.clients_num() == 0:
            return self.STATE_VOFA_CLIENT_NOT_AVAILABLE
        if not self.data_available:
            return self.STATE_DATA_NOY_AVAILABLE
        if len(self.attrIds) == 0:
            return self.STATE_ATTRIBUTE_NOT_CONFIGURED
        return self.STATE_OK

    def log(self, msg):
        if self.uiLog:
            self.uiLog.log(msg)    
    
    def is_enabled(self):
        return self.data_available

    def enable(self):
        self.data_available = True 
    
    def disable(self):
        self.data_available = False

    def on_start(self):
        self.log("启动VOFA数据服务@%s, 周期:%.3f秒" % (self.info(), self.period))

    def on_stop(self):
        self.log("停止VOFA数据服务@%s" % self.info())

    def on_client_connect(self, client: Client):
        self.add_client(client)
        self.log("VOFA客户端(%s)连接成功" % client.info())

    def on_client_disconnect(self, client):
        self.remove_client(client)
        self.log("VOFA客户端(%s)连接断开" % client.info())

    def get_data_debug(self)->List[float]:
        ## 发送数据的条件为：
        ## enable = True
        ## 客户端已连接 
        ## 有效属性id数要大于0
        ## 产生一个sin函数 
        # if not self.data_available:
        #     return []
        if self.clients_num() == 0:
            return []

        self.test_counter1 += (math.pi / 100)
        data = []
        v1 = 0
        if math.sin(self.test_counter1) >= 0.5:
            v1 = 1
        
        v2 = math.sin(self.test_counter1)
        v3 = math.cos(self.test_counter1)
        data.append(v1)
        data.append(v2)
        data.append(v3)
        return data

    def get_data(self)->List[float]:
        if self.get_state() != self.STATE_OK:
            return []
        values = []
        for v in self.attrIds:
            values.append(self.db.get(v, default_value=0.0))
        return values

    def make_frame(self, data:List[float])->bytes:
        f = bytes()
        for b in data:
            f += struct.pack('< f', b)
        f += struct.pack('B', 0)
        f += struct.pack('B', 0)
        f += struct.pack('B', 0x80)
        f += struct.pack('B', 0x7f)
        return f
    
    def timer_handle(self):
        #data = self.get_data_debug()
        data = self.get_data()
        if len(data) == 0:
            if self.transmiting or self.begin:
                self.transmiting = False 
                state = self.get_state()
                if state == self.STATE_ATTRIBUTE_NOT_CONFIGURED:
                    self.log("VOFA属性未设置，数据发送暂停")
                elif state == self.STATE_DATA_NOY_AVAILABLE:
                    self.log("VOFA数据不可用，数据发送暂停")
                else :
                    self.log("VOFA客户端未连接，数据发送暂停")
        else:
            ## 不加延时会导致 某些线程很卡
            if not self.transmiting:
                self.transmiting = True 
                self.log("VOFA数据发送中...")
            self.broadcast(self.make_frame(data))
        self.begin = False
            
    def start(self) -> bool:
        ret = super().start()
        if ret:
           self.timer.start(self.period)
    
    def stop(self):
        self.timer.stop()
        super().stop()
